//
// Created by 26090 on 24-12-18.
//

#include "wit_process.h"

#include <cmsis_os2.h>

#include "bsp_usart.h"
#include "wit_c_sdk.h"


#define ACC_UPDATE		0x01
#define GYRO_UPDATE		0x02
#define ANGLE_UPDATE	0x04
#define MAG_UPDATE		0x08
#define READ_UPDATE		0x80


static USARTInstance *wit_usart_instance;

#define WIT_MAX_BUFF 512

volatile uint8_t s_cDataUpdate = 0, s_cCmd = 0xff;

void wit_decode(struct _usart_instance* _handle);

void SensorUartSend(uint8_t* p_uc_data, uint32_t ui_len);

void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum);

void Delayms(uint16_t uc_ms);

void wit_init(UART_HandleTypeDef *uart_instance)
{
    USART_Init_Config_s config;
    config.module_callback = wit_decode;
    config.recv_buff_size = WIT_MAX_BUFF;
    config.usart_handle = uart_instance;
    config.tx_fifo_mode = USART_FIFO_DISABLE;
    config.duplex_mode = USART_DUPLEX_FULL;
    wit_usart_instance = USARTRegister(&config);

    WitInit(WIT_PROTOCOL_MODBUS, 0x50);
    WitSerialWriteRegister(SensorUartSend);
    WitRegisterCallBack(CopeSensorData);
    WitDelayMsRegister(Delayms);
}

void wit_process(void)
{
    WitReadReg(AX, 12);
    static float fAcc[3], fGyro[3], fAngle[3];
    if(s_cDataUpdate)
    {
        for(int i = 0; i < 3; i++)
        {
            fAcc[i] = sReg[AX+i] / 32768.0f * 16.0f;
            fGyro[i] = sReg[GX+i] / 32768.0f * 2000.0f;
            fAngle[i] = sReg[Roll+i] / 32768.0f * 180.0f;
        }
    }
}

void wit_decode(struct _usart_instance* _handle)
{
    uint8_t *recv_buff = _handle->recv_buff;
    uint16_t recv_size = _handle->recv_size;
    WitSerialDataInArray(recv_buff, recv_size);
}

void SensorUartSend(uint8_t* p_uc_data, uint32_t ui_len)
{
    USARTSend(wit_usart_instance,p_uc_data,ui_len,USART_TRANSFER_BLOCKING);
}

void CopeSensorData(uint32_t uiReg, uint32_t uiRegNum)
{
    int i;
    for(i = 0; i < uiRegNum; i++)
    {
        switch(uiReg)
        {
            //            case AX:
            //            case AY:
        case AZ:
            s_cDataUpdate |= ACC_UPDATE;
            break;
            //            case GX:
            //            case GY:
        case GZ:
            s_cDataUpdate |= GYRO_UPDATE;
            break;
            //            case HX:
            //            case HY:
        case HZ:
            s_cDataUpdate |= MAG_UPDATE;
            break;
            //            case Roll:
            //            case Pitch:
        case Yaw:
            s_cDataUpdate |= ANGLE_UPDATE;
            break;
        default:
            s_cDataUpdate |= READ_UPDATE;
            break;
        }
        uiReg++;
    }
}

void Delayms(uint16_t uc_ms)
{
    osDelay(uc_ms);
}
